/***************************************************************************
 创建者: 华磊
 开始时间: 2019.1.14
 copyright: (C) 华友高科
 ***************************************************************************/
#include "cu1positioncalculate.h"

Cu1PositionCalculate::Cu1PositionCalculate(double distanceOfTwoLaserIn,
                                    double distanceOfLaserAndVehicleCenterIn,
                                        double distanceOffsetOfCameraIn, double ratioOfPixIn)
{
    distanceOfTwoLaser=distanceOfTwoLaserIn;
    distanceOfLaserAndVehicleCenter=distanceOfLaserAndVehicleCenterIn;
    distanceOffsetOfCamera=distanceOffsetOfCameraIn;
    ratioOfPix=ratioOfPixIn;
}

int Cu1PositionCalculate::transSensorData2VehiclePosition(double laser1Length, double laser2Length,
                                                 double imageOffset, VehiclePosition &positionOut)
{
    positionOut.rotate=atan2(distanceOfTwoLaser,laser2Length-laser1Length);//正负号是否对？
    double middleLength=distanceOfLaserAndVehicleCenter+(laser2Length+laser1Length)/2.0;
    positionOut.y=-middleLength*sin(positionOut.rotate);

    double imageRatioDistance=(laser2Length+laser1Length)/2.0+distanceOffsetOfCamera;
    double actualImageOffset=ratioOfPix*imageOffset*imageRatioDistance;
    positionOut.x=actualImageOffset;//简化为垂直角度，忽略角度偏差带来的误差
    //使用了近似值，，，二维码附近有一段偏差距离没有考虑
//    positionOut.x=-(middleLength-actualImageOffset)*cos(positionOut.rotate);//垂直时误差很大。
    return 1;
}
